#ifndef BSPLINE_HPP_
#define BSPLINE_HPP_

#include "/usr/include/eigen3/Eigen/Dense"
#include <Eigen/src/LU/FullPivLU.h>
#include <algorithm>
#include <cstddef>
#include <eigen3/Eigen/src/Core/Matrix.h>
#include <stdexcept>
#include <vector>
#include "matplotlibcpp.h"

namespace plt = matplotlibcpp;

class BSpline{
    public:
        //typedef Eigen::VectorXd vector_type;
        typedef std::vector<double> vector_type;
        typedef Eigen::MatrixXd matrix_type;
        enum bd_type{
            complete = 1,
            natural = 2,
            periodic = 3
        };

    private:
        int degree=3;  // degree of B-Spline 
        bd_type boundary=natural;   // three type of boundary condition for cubic b_spline
        vector_type knots;  // knots to compute the basic funtion
        vector_type xData;  // x interpolate value
        vector_type yData;  // y interpolate value
        Eigen::VectorXd control_points;  // the coefficient of the basic function
        double l_bd=0, r_bd=0;  // boundary conditon for cubic b_spline
        
    public:
        BSpline(int deg=3): degree(deg){}
        BSpline(int deg, vector_type xValue, vector_type yValue, bd_type bd=natural, double l_val=0, double r_val=0):
            degree(deg), xData(xValue), yData(yValue), boundary(bd), l_bd(l_val), r_bd(r_val){
                setupSpline();
            } 
        BSpline(const BSpline &) = default;
        BSpline(BSpline &&) = default;
        BSpline &operator=(const BSpline &) = default;
        BSpline &operator=(BSpline &&) = default;

        void setDegree(int deg);
        void setKnots(vector_type x);
        void setData(vector_type x, vector_type y);
        void setBoundary(bd_type bd, double l_val=0, double r_val=0);
        void generateKnots();
        void setupSpline();
        double interpolate(double t);
        void draw(std::string file_name);

        void cardinalQuadratic(int xStart, int xEnd, vector_type yValue);

        size_t get_closest_left_idx(double x) const;
        double basicFun(int i, int n, double t);
};

inline void BSpline::setDegree(int deg){
    degree = deg;
}

inline void BSpline::setKnots(vector_type x){
    knots = x;
}

inline void BSpline::setData(vector_type x, vector_type y){
    xData = x;
    yData = y;
}

inline void BSpline::setBoundary(bd_type bd, double l_val, double r_val){
    boundary = bd;
    l_bd = l_val;
    r_bd = r_val;
}

inline void BSpline::generateKnots() {
    int n = xData.size();
    knots.resize(n + 2*degree);
    for (int i = 0; i < degree + 1; ++i) {
        knots[i] = xData[0];
    }
    for (int i = degree + 1; i < n + degree; ++i) {
        knots[i] = xData[i-degree];
    }
    for (int i = n + degree; i < n + 2*degree; ++i) {
        knots[i] = knots[n + degree - 1];
    }
}

inline void BSpline::setupSpline() {
    generateKnots();
    int n = xData.size();
    matrix_type N = matrix_type::Zero(n + degree - 1, n + degree - 1);
    Eigen::VectorXd F = Eigen::VectorXd::Zero(n + degree - 1);

    if (degree==3) {      
        //填写矩阵N
        for (int i=degree+1;i<n+degree+1;++i)
        {
            int k = i - degree + 2;
            //basicFtn is nonzero on degree many funcitons
            for (int j=i-degree;j<i;++j)
            {
                N(i-degree,j-1) = basicFun(j, degree, knots[k]);
            }
        }
        //填写F
        for(int i=1;i<n+1;i++){
            F(i) = yData[i-1];
        }
        
        //边界条件
        if (boundary == complete) {
            N(0,0) = -1.0/(knots[4]-knots[1])*basicFun(2, 2, knots[1]);
            N(0,1) = 1.0/(knots[4]-knots[1])*basicFun(2, 2, knots[1])
                             - 1.0/(knots[5]-knots[2])*basicFun(3, 2, knots[1]);
            N(n+1,n) = 1.0/(knots[n+3]-knots[n])*basicFun(n+1, 2, knots[n+3])
                             - 1.0/(knots[n+4]-knots[n+1])*basicFun(n+2, 2, knots[n+3]);
            N(n+1,n+1) = 1.0/(knots[n+4]-knots[n+1])*basicFun(n+2, 2, knots[n+3]);

            F(0) = l_bd/degree;
            F(n+1) = r_bd/degree;
        } else if (boundary == natural) {
            N(0,0) = 1.0/(knots[4]-knots[1]);
            N(0,1) = -1.0/(knots[4]-knots[1])
                            -1.0/(knots[5]-knots[2]);
            N(0,2) = 1.0/(knots[5]-knots[2]);
            N(n+1,n-1) = 1.0/(knots[n+3]-knots[n]);
            N(n+1,n) = -1.0/(knots[n+3]-knots[n])
                             - 1.0/(knots[n+4]-knots[n+1]);
            N(n+1,n+1) = 1.0/(knots[n+4]-knots[n+1]);
            // N(0,0) = 1.0/(knots[4]-knots[1])/(knots[4]-knots[2])*basicFun(3, 1, knots[1]);
            // N(0,1) = -1.0/(knots[4]-knots[1])/(knots[4]-knots[2])*basicFun(3, 1, knots[1])
            //                 -1.0/(knots[5]-knots[2])/(knots[4]-knots[2])*basicFun(3, 1, knots[1]);
            // N(0,2) = 1.0/(knots[5]-knots[2])/(knots[4]-knots[2])*basicFun(3, 1, knots[1]);
            // N(n+1,n-1) = 1.0/(knots[n+3]-knots[n])/(knots[n+3]-knots[n+1])*basicFun(n+2, 1, knots[n+3]);
            // N(n+1,n) = -1.0/(knots[n+3]-knots[n])/(knots[n+3]-knots[n+1])*basicFun(n+2, 1, knots[n+3])
            //                  - 1.0/(knots[n+4]-knots[n+1])/(knots[n+3]-knots[n+1])*basicFun(n+2, 1, knots[n+3]);
            // N(n+1,n+1) = 1.0/(knots[n+4]-knots[n+1])/(knots[n+3]-knots[n+1])*basicFun(n+2, 1, knots[n+3]);
        } else {
            N(0,0) = -1.0/(knots[4]-knots[1])*basicFun(2, 2, knots[1]);
            N(0,1) = 1.0/(knots[4]-knots[1])*basicFun(2, 2, knots[1])
                 - 1.0/(knots[5]-knots[2])*basicFun(3, 2, knots[1]);
            N(0,n) = (1.0/(knots[n+3]-knots[n])*basicFun(n+1, 2, knots[n+3])
                 - 1.0/(knots[n+4]-knots[n+1])*basicFun(n+2, 2, knots[n+3]))*-1;
            N(0,n+1) = (1.0/(knots[n+4]-knots[n+1])*basicFun(n+2, 2, knots[n+3]))*-1;
            N(n+1,0) = 1.0/(knots[4]-knots[1])/(knots[4]-knots[2])*basicFun(3, 1, knots[1]);
            N(n+1,1) = -1.0/(knots[4]-knots[1])/(knots[4]-knots[2])*basicFun(3, 1, knots[1])
                -1.0/(knots[5]-knots[2])/(knots[4]-knots[2])*basicFun(3, 1, knots[1]);
            N(n+1,2) = 1.0/(knots[5]-knots[2])/(knots[4]-knots[2])*basicFun(3, 1, knots[1]);
            N(n+1,n-1) = (1.0/(knots[n+3]-knots[n])/(knots[n+3]-knots[n+1])*basicFun(n+2, 1, knots[n+3]))*-1;
            N(n+1,n) = (-1.0/(knots[n+3]-knots[n])/(knots[n+3]-knots[n+1])*basicFun(n+2, 1, knots[n+3])
                 - 1.0/(knots[n+4]-knots[n+1])/(knots[n+3]-knots[n+1])*basicFun(n+2, 1, knots[n+3]))*-1;
            N(n+1,n+1) = (1.0/(knots[n+4]-knots[n+1])/(knots[n+3]-knots[n+1])*basicFun(n+2, 1, knots[n+3]))*-1;
        }
        // solve N*control_points = F
        Eigen::FullPivLU<Eigen::MatrixXd> lu;
        lu.compute(N);
        control_points = lu.solve(F);
    }else {
        for (int i=0;i<n;++i)
        {
            //basicFtn is nonzero on degree many funcitons
            for (int j=i;j<i+degree;j++)
            {
                N(i,j) = basicFun(j+1, degree, knots[i+degree]);
            }
        }

        //fill in F
        for(int i=0;i<n;i++){
            F(i) = yData[i];
        }

        Eigen::FullPivLU<Eigen::MatrixXd> lu;
        lu.compute(N);
        control_points = lu.solve(F);
    }
}

inline void BSpline::cardinalQuadratic(int xStart, int xEnd, vector_type yValue){
    degree = 2;
    int n = xEnd - xStart;
    if (n != yValue.size()-2) {
        throw std::invalid_argument("ivalid data");
    }
    knots.clear();
    xData.clear();
    for (int i=xStart-2; i<=xEnd+2; i++){
        knots.push_back(i);
    }
    for (int i=xStart; i<=xEnd; i++){
        xData.push_back(i);
    }
    matrix_type M = matrix_type::Zero(n,n);
    Eigen::VectorXd b = Eigen::VectorXd::Zero(n);
    M(0,0)=5;
    M(0,1)=1;
    M(n-1,n-1)=5;
    M(n-1,n-2)=1;
    b(0)=8*yValue[1]-2*yValue[0];
    b(n-1)=8*yValue[n]-2*yValue[n+1];
    for (int i=1; i<n-1;i++){
        M(i,i-1)=1;
        M(i,i)=6;
        M(i,i+1)=1;
        b(i)=8*yValue[i+1];
    }
    Eigen::FullPivLU<matrix_type> lu;
    lu.compute(M);
    Eigen::VectorXd a = lu.solve(b);
    control_points.resize(n+2);
    for (int i=0 ; i<n; i++) {
        control_points(i+1) = a(i);
    }
    control_points(0) = 2*yValue[0]-control_points(1);
    control_points(n+1) = 2*yValue[n+1]-control_points(n);
}

inline double BSpline::interpolate(double t) {
    size_t idx = get_closest_left_idx(t);
    double tmp = 0; // 初始化变量是重中之重，不然在创建过多对象时会分配到已经构造但是没有析构的内存
    for(int i=0;i<degree+1;i++){
        tmp = tmp + control_points(idx+i)*basicFun(idx+i+1, degree, t);
    }
    return tmp;
        //  control_points(idx)*basicFun(idx+1, degree, t) 
        // + control_points(idx+1)*basicFun(idx+2, degree, t) 
        // + control_points(idx+2)*basicFun(idx+3, degree, t)
        // + control_points(idx+3)*basicFun(idx+4, degree, t);
}

inline void BSpline::draw(std::string file_name){
    std::vector<double> x,y;
    double stepsize = 0.01;
    double current = xData[0];
    while(current <= xData[xData.size()-1]){
        x.push_back(current);
        y.push_back(interpolate(current));
        current += stepsize;
    }
    plt::figure_size(1200, 780);
    plt::plot(x, y); 
    plt::plot(xData, yData, "o");
    plt::save(file_name, 300);
    plt::show();
}

inline size_t BSpline::get_closest_left_idx(double x) const {
    auto it = std::upper_bound(xData.begin(), xData.end()-1, x);
    // *it > x
    size_t idx = std::max(static_cast<int>(it - xData.begin()) - 1, 0);
    // m_x[idx] <= x
    return idx;
}

inline double BSpline::basicFun(int i, int n, double t){
    if(n == 0){
        if (i==degree+1) {
            if(t >= knots[i-1] && t <= knots[i]){
                return 1;
            }else{
                return 0;
            }          
        }else if(t > knots[i-1] && t <= knots[i]){
            return 1;
        }else{
            return 0;
        }
        // if (t>knots[i-1] && t<=knots[i]) {
        //     return 1;
        // }else {
        //     return 0;
        // }          
    }else if(knots[i+n-1]==knots[i-1]&&knots[i+n]==knots[i]){
        return 0;
    }else if (knots[i+n-1]==knots[i-1]) {
        return (knots[i+n]-t)/(knots[i+n]-knots[i])*basicFun(i+1, n-1, t);
    }else if (knots[i+n]==knots[i]) {
        return (t-knots[i-1])/(knots[i+n-1]-knots[i-1])*basicFun(i, n-1, t);
    }else {
        return (t-knots[i-1])/(knots[i+n-1]-knots[i-1])*basicFun(i, n-1, t) 
            + (knots[i+n]-t)/(knots[i+n]-knots[i])*basicFun(i+1, n-1, t);
    }
    
}
#endif